#include "imu_integration.h"

ImuIntegration::ImuIntegration(const double &init_bias) {
    init_bias_ = init_bias;
}

void ImuIntegration::Integrate(const bool &is_init_bias, const Eigen::Vector3d& last_imu, const Eigen::Vector3d& curr_imu, const double& dt, State &state) {
    state.last_imu = last_imu;
    state.curr_imu = curr_imu;
    
    Eigen::Vector3d last_imu_unbias(last_imu[0], last_imu[1], last_imu[2]);
    Eigen::Vector3d curr_imu_unbias(curr_imu[0], curr_imu[1], curr_imu[2]);
    if(is_init_bias) {
        last_imu_unbias[2] -= init_bias_;
        curr_imu_unbias[2] -= init_bias_;        
    } else {
        last_imu_unbias[0] -= state.ba[0];
        curr_imu_unbias[0] -= state.ba[0];
        last_imu_unbias[1] -= state.ba[1];
        curr_imu_unbias[1] -= state.ba[1];
        last_imu_unbias[2] -= state.bg;
        curr_imu_unbias[2] -= state.bg;
    }

    // std::cout << "dt ============= : " << dt << ", " << init_bias_ << std::endl;
    Eigen::Vector3d imu_unbias_hat = 0.5 * (last_imu_unbias + curr_imu_unbias);

    double dtheta = imu_unbias_hat(2) * dt;

    // Eigen::Matrix<double,2,2> R = math_utils::rotation(dtheta);
    Eigen::Matrix<double,2,2> R = math_utils::rotation(state.theta);

    // update q, also plus dtheta, reason of this data
    state.theta += dtheta;
    state.theta = atan2(sin(state.theta), cos(state.theta));

    
    // std::cout << "dtheta ============= : " << dt << ", " << dtheta << std::endl;
    // std::cout << "state.theta ============= : " << dt << ", " << state.theta << std::endl;

    // update p 
    Eigen::Vector2d acc_hat(imu_unbias_hat[0], imu_unbias_hat[1]);
    state.p += state.v * dt + 0.5 * R.transpose() * acc_hat * dt * dt;

    // update v 
    state.v += R.transpose() * acc_hat * dt;

    // update F matrix
    Eigen::Matrix<double, 8, 8> F = Eigen::Matrix<double, 8 ,8>::Identity();
    // Derivative of Pose
    F.block<2, 2>(0, 0) = Eigen::Matrix<double, 2 ,2>::Identity(); // dp/dp
    F.block<2, 2>(0, 2) = dt * Eigen::Matrix<double, 2 ,2>::Identity(); // dp/dv
    Eigen::Matrix<double, 2, 2> dR = Eigen::Matrix<double, 2 ,2>::Identity();
    dR(0, 0) = R(0, 1);
    dR(1, 1) = dR(0, 0);  
    dR(0, 1) = -R(0, 0); 
    dR(1, 0) = R(0, 0); 

    F.block<2, 1>(0, 4) = 0.5 * dR * acc_hat * dt * dt; // dp/dtheta
    F.block<2, 2>(0, 5) = -0.5 * R * dt * dt; // dp/dba
    F.block<2, 1>(0, 7) = Eigen::Matrix<double, 2 ,1>::Zero(); // dp/dbg
    // Derivative of V
    F.block<2, 2>(2, 0) = Eigen::Matrix<double, 2 ,2>::Zero(); // dv/dp
    F.block<2, 2>(2, 2) = Eigen::Matrix<double, 2 ,2>::Identity(); // dv/dv
    F.block<2, 1>(2, 4) = dR * acc_hat * dt; // dv/dtheta
    F.block<2, 2>(2, 5) = -R * dt; // dv/dba
    F.block<2, 1>(2, 7) = Eigen::Matrix<double, 2 ,1>::Zero(); // dv/dbg
    // Derivative of Q
    F.block<1, 4>(4, 0) = Eigen::Matrix<double, 1, 4>::Zero(); // dQ/dp/dv
    F(4, 4) = 1; // dQ/dtheta
    F.block<1, 2>(4, 5) = Eigen::Matrix<double, 1, 2>::Zero(); // dQ/dba
    F(4, 7) = -dt; // dQ/dbg
    // Derivative of bias
    F.block<3, 5>(5, 0) = Eigen::Matrix<double, 3, 5>::Zero();
    F.block<3, 3>(5, 5) = Eigen::Matrix<double, 3, 3>::Identity(); 

    // update update noise
    Eigen::Matrix<double, 8, 8> G = std::pow(0.001,2) * Eigen::Matrix<double, 8, 8>::Identity();
    G(0,0) = std::pow(0.1,2);
    G(1,1) = std::pow(0.1,2);
    G(2,2) = std::pow(0.1,2);
    G(3,3) = std::pow(0.1,2);

    state.cov = F * state.cov * F.transpose() + G;

    std::cout << "state.theta ============= : " << state.theta << std::endl;
    std::cout << "state.p ============= : " << state.p.transpose() << std::endl;
    std::cout << "state.v ============= : " << state.v.transpose() << std::endl;
    std::cout << "state.ba ============= : " << state.ba.transpose() << std::endl;
    std::cout << "state.bg ============= : " << state.bg << std::endl;
    std::cout << "state.cov ============= : \n" << state.cov << std::endl;

    state.dt = dt;
}
